package com.yuneec.mavlink;

import android.util.Log;

import com.MAVLink.MAVLinkPacket;
import com.MAVLink.Parser;
import com.MAVLink.yuneec.msg_mav_rc_cmd;
import com.MAVLink.yuneec.msg_mav_rc_cmd_ack;
import com.yuneec.droneservice.DroneService;
import com.yuneec.droneservice.ServiceManager;
import com.yuneec.droneservice.command.BaseCommand;
import com.yuneec.droneservice.command.MAV_RC_COMMAND;
import com.yuneec.droneservice.model.HardwareEvent;
import com.yuneec.droneservice.parse.DroneFeedback;
import com.yuneec.droneservice.protocol.ProtocolPlugin;
import com.yuneec.droneservice.protocol.analysis.ByteBuf;
import com.yuneec.droneservice.request.IRequest;
import com.yuneec.droneservice.request.IResponseHandler;
import com.yuneec.droneservice.request.OnListener;
import com.yuneec.droneservice.request.OnListenerHandler;

import java.lang.ref.WeakReference;

/**
 * Created by zd.zhang on 2017/9/12.
 */

public class MavLinkProtocolPlugin implements ProtocolPlugin {
    public static final int L_MAVLINK = 0x100;
    public static final int MAVLINK_MSG_ID = 10011;
    ///////////////////////////////////////////////////////////////////
    private OnListenerHandler<DroneFeedback> droneFeedbackListener;
    private OnListenerHandler<HardwareEvent> hardwareEventListener;
    private OnListenerHandler<byte[]> payloadByPassListener;
    private WeakReference<DroneService.DroneBinder> binderReference;
    /////////////////////////////////////////////////////////////////////

    @Override
    public IResponseHandler handleResponseBuf(byte[] data, int off, int len) {
        Parser parser = new Parser();
        try {
            for (int i = 0; i < len; i++) {
                MAVLinkPacket pack = parser.mavlink_parse_char(data[off + i] & 0xFF);
                if (pack != null) {
                    switch (pack.msgid) {
                        case msg_mav_rc_cmd_ack.MAVLINK_MSG_ID_MAV_RC_CMD_ACK:
                            return pack.unpack();
                        case msg_mav_rc_cmd.MAVLINK_MSG_ID_MAV_RC_CMD:
                            msg_mav_rc_cmd cmd = (msg_mav_rc_cmd) (pack.unpack());
                            switch (cmd.command) {
                                case MAV_RC_COMMAND.MAV_RC_MSG_TELEMETRY_DATA:
                                    return handleDroneFeedback(cmd);
                                case MAV_RC_COMMAND.MAV_RC_MSG_BYPASS_EX_PACKET:
                                    return handPayloadByPass(cmd);
                                case MAV_RC_COMMAND.MAV_RC_MSG_REPORT_EVENT:
                                    return handControllerEvent(cmd);
                            }
                            return null;
                        default:
                            return new OnListenerHandler<>(L_MAVLINK, new ByteBuf(data, off, len), binderReference.get());
                    }
                }
            }
        } catch (Exception e) {
            Log.e("zzd_aoa", String.format("toResponse error:%s", e.getMessage()));
        }
        return null;
    }

    private IResponseHandler handleDroneFeedback(msg_mav_rc_cmd ack) {
        if (ack == null || ack.params == null || ack.params.length < 37) {
            return null;
        }
        if (droneFeedbackListener == null) {
            DroneService.DroneBinder binder = binderReference.get();
            if (binder == null) {
                return null;
            }
            droneFeedbackListener = new OnListenerHandler<>(OnListener.L_DRONE_FEEDBACK, new DroneFeedback(), binder);
        }
        DroneFeedback droneFeedback = droneFeedbackListener.get();
        droneFeedback.latitude = (BaseCommand.uIntArrayToInt(ack.params, 2) * 1e-7f);
        droneFeedback.longitude = (BaseCommand.uIntArrayToInt(ack.params, 6) * 1e-7f);
        droneFeedback.altitude = (BaseCommand.uIntArrayToInt(ack.params, 10) * 0.01f);

        float vx = BaseCommand.uIntArrayToShort(ack.params, 14) * 0.01f;
        float vy = BaseCommand.uIntArrayToShort(ack.params, 16) * 0.01f;
        float vz = BaseCommand.uIntArrayToShort(ack.params, 18) * 0.01f;

        droneFeedback.tas = (float) Math.sqrt(vx * vx + vy * vy + vz * vz);
        droneFeedback.vSpeed = vz;
        droneFeedback.hSpeed = (float) Math.sqrt(vx * vx + vy * vy);

        droneFeedback.gpsUsed = ((ack.params[20] & 0x80) >> 7) > 0;
        droneFeedback.fixType = ((ack.params[20] & 0x60)) >> 5;
        droneFeedback.satellitesNum = ack.params[20] & 0x1f;
        droneFeedback.voltage = (ack.params[21] & 0xff) * 0.1f + 5;
        droneFeedback.current = (ack.params[22] & 0xff) * 0.5f;

        droneFeedback.roll = BaseCommand.uIntArrayToUShort(ack.params, 23) * 0.01f;
        droneFeedback.pitch = BaseCommand.uIntArrayToUShort(ack.params, 25) * 0.01f;
        droneFeedback.yaw = BaseCommand.uIntArrayToUShort(ack.params, 27) * 0.01f;

        droneFeedback.motorStatus = ack.params[29] & 0xff;
        droneFeedback.imuStatus = ack.params[30] & 0xff;
        droneFeedback.pressCompassStatus = ack.params[31] & 0xff;
        droneFeedback.fMode = ack.params[32] & 0xff & 0x7f;
        droneFeedback.vehicleType = ack.params[33] & 0xff;
        droneFeedback.errorFlags1 = ack.params[34] & 0xff;
        droneFeedback.gpsAccH = (ack.params[35] & 0xff) * 0.05f;
        droneFeedback.fskRssi = ack.params[36];
        return droneFeedbackListener;
    }

    private IResponseHandler handPayloadByPass(msg_mav_rc_cmd ack) {
        if (ack != null && ack.params != null && ack.params.length > 2) {
            if (payloadByPassListener == null) {
                DroneService.DroneBinder binder = binderReference.get();
                if (binder != null) {
                    payloadByPassListener = new OnListenerHandler<>(OnListener.L_PAYLOAD_BY_PASS, null, binder);
                } else {
                    return null;
                }
            }
            byte[] buf = new byte[ack.params.length];
            for (int i = 0; i < buf.length; i++) {
                buf[i] = (byte) (ack.params[i] & 0xFF);
            }
            payloadByPassListener.set(buf);
            return payloadByPassListener;
        }
        return null;
    }

    private IResponseHandler handControllerEvent(msg_mav_rc_cmd ack) {
        if (ack == null || ack.params == null || ack.params.length < 2) {
            return null;
        }
        if (hardwareEventListener == null) {
            DroneService.DroneBinder binder = binderReference.get();
            if (binder == null) {
                return null;
            }
            hardwareEventListener = new OnListenerHandler<>(OnListener.L_CONTROLLER_FEEDBACK, new HardwareEvent(), binder);
        }
        HardwareEvent hardwareEvent = hardwareEventListener.get();
        hardwareEvent.setHwId(ack.params[0] & 0xff);
        hardwareEvent.setValue(ack.params[1] & 0xff);
        return hardwareEventListener;
    }

    @Override
    public byte[] toRawData(IRequest request) {
        return BaseCommand.class.cast(request).toRawData();
    }

    @Override
    public int getPort() {
        return MAVLINK_MSG_ID;
    }

    @Override
    public void init(DroneService.DroneBinder mBinder) {
        binderReference = new WeakReference<>(mBinder);
    }

    public static void clearMavLinkReceive() {
        ServiceManager.getInstance().clearListener(L_MAVLINK);
    }

    public static void addMavLinkReceive(OnListener<ByteBuf> listener) {
        ServiceManager.getInstance().addListener(L_MAVLINK, listener);
    }

    public static void removeMavLinkReceive(OnListener<ByteBuf> listener) {
        ServiceManager.getInstance().removeListener(L_MAVLINK, listener);
    }
}
